#include "Matrix.h"
#include "PArray.h"
#include "Triangle.h"
#include "TopologicalModel.h"
#include <cmath>

const Real INFTY = 1e8;

TriIntersection::TriIntersection()
{
	victim_ = 0;
	distance_ = INFTY;
}

Vector3 TriIntersection::Point() const
{
	return ray_.Point(distance_);
}

TriIntersection IntersectTriangles(Ray ray, TopologicalModel *mod, bool dyn)
{
	TriIntersection minimal;
	minimal.ray_ = ray;

	set<Triangle*>::const_iterator bt(mod->OuterFacesBegin());
	set<Triangle*>::const_iterator et(mod->OuterFacesEnd());
	for(; bt!=et; bt++)
	{
		TriIntersection inter = (*bt)->IntersectWithRay(ray, dyn);
		if (inter.victim_ && minimal.distance_ > inter.distance_)
		{
			minimal = inter;
		}
	}
	return minimal;
}

Ray::Ray()
{

}

Vector3 Ray::Point(Real t) const
{
	Vector3 v(origin_);
	v += t*dir_;
	return v;
}

/*
should move Topological stuff out of here.
*/

void Matrix2Multiply (Real *d, Real m[2][2], Real *v)
{
	for (int i=0; i < 2; i++)
	{
		*d = 0;
		for (int j=0; j < 2; j++)
			*d += m[i][j] * v[j];
		d++;
	}
}

void InvertMatrix2 (Real dst[2][2], Real src[2][2])
{
	Real d0  = src[0][0];
	Real d1 = src[1][1];
	Real det = d0*d1 - src[1][0]*src[0][1] ;

	dst[0][0] = d1;
	dst[1][1] = d0;
	dst[0][1] = -src[0][1];
	dst[1][0] = -src[1][0];

	for (int i=0; i < 2; i++)
		for (int j=0; j < 2; j++)
			dst[i][j] /= det;
}

/*
Return in COEF X expressed in barycentric coords. X should be on the
triangle.
*/
void BarycentricCoordinates (Vector3 const * vertices,
						 Real *coef, Vector3 x) 
{
	Vector3 v1 = vertices[1] - vertices[0];
	Vector3 v2 = vertices[2] - vertices[0];
	x = x - vertices[0];

	Real d1d1 = v1 * v1;
	Real d1d2 = v1 * v2;
	Real d2d2 = v2 * v2;            

	Real mat[2][2] = {{ d1d1, d1d2}, {d1d2,d2d2} };
	Real rhs[] ={x  * v1, x * v2};

	Real inv[2][2];
	InvertMatrix2 (inv, mat);

	Matrix2Multiply (coef + 1, inv, rhs);

	coef[0] = 1 - (coef[1] + coef[2]);
}


bool IntersectTriangleWithRay (Vector3 const *v, Ray ray, Real *dist) 
{
	Vector3 v0 = v[0];
	Vector3 v1 = v[1];
	Vector3 v2 = v[2];

	v0 -= v2;
	v1 -= v2;
	Vector3 normal = Vector3::Cross (v0, v1);

	Real off = v2 * normal;

	Real dot = normal * ray.dir_;
	if (fabs (dot) < 1e-8)	// ugh
		return false;

	*dist = ( off -  ray.origin_ * normal)  / dot;

	if (*dist < 0)
		return false;

	Real coef[3];
	Vector3 point = ray.dir_ * *dist + ray.origin_;
	BarycentricCoordinates (v, coef, point);

	return  (coef[0] >= 0 && coef[1] >= 0 && coef[1] + coef[0] <= 1.0);
}

